/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.techhighteam675.robot2012;

/**
 *
 * @author Noah Dove
 */

public class MathExtension
{   
    public static double SolveSecondDegree(double a, double b, double c, boolean plus)
    {
		double x;
		if (plus)
                {
			x = (-b + Math.sqrt(b*b - (4*a*c)))/(2*a);
                }
		else
                {
			x = (-b - Math.sqrt(b*b - (4*a*c)))/(2*a);
                }
		return x;
	}
	
    public static double SolveThirdDegree(double a, double b, double c, double d)
        {
		double x;
		double term1 = (-b*-b*-b)/(27*a*a*a);
		double term2 = b*c /(6*a*a);
		double term3 = -d/(2*a);
		double term4 = c/(3*a);
		double term5 = -b*b/(9*a*a);
		x = cuberoot(((term1+term2+term3)+Math.sqrt((term1+term2+term3)+(cube(term4+term5)))))
		  + cuberoot(((term1+term2+term3)-Math.sqrt((term1+term2+term3)+(cube(term4+term5)))))
		  - (b/3*a);
		return x;
	}
	
    public static double cuberoot (double a){
		double x = pow(a, 1/3);
		return x;
	}
	
    public static double cube(double a){
		double x = a*a*a;
		return x;
	}
        
    public static double sgn(double x) 
    {   
        return x == 0 ? 0 : x/Math.abs(x);
    }
    
    /**
     Calculates the hypotenuse of a right triangle with legs a and b.
     */
    public static double hypot(double a, double b)
    {
        return Math.sqrt((a*a) + (b*b));
    }
    
    /**
     Calculates the leg of a right triangle with leg b and hypotenuse c.
     */
    public static double leg(double c, double a)
    {
        return Math.sqrt((c*c) - (a*a));
    }
    
    /**
     Raises z to the power of n.
     */
    public static double pow(double z, int n)
    {
        double result = 1;
        
        while(n > 0)
        {
            result *= z;
            n--;
        }
        
        return result;
    }
    
    public static final int INV_TRIG_OPERATION_REP_N = 5;
    
    /**
     Calculates the arcsine of z (in radians) using an approximation of an infinite sum. 
     * The sum is calculated using NV_TRIG_OPERATION_REP_N for the number of repetitions.
     * Matches the function in later versions of java to the first 6 or so decimal places.<p> 
     * All other inverse trigonometric functions in this file simply convert their arguments the 
     * appropriate sides and call this one for the actual trig operations.
     */
    public static double rawAsin(double z)
    {
        if(Math.abs(z) > 1)
        {
            throw new IllegalArgumentException("Argument out of range: must be in between 1 and -1");
        }
        
	double result = z;
		
	int tempn;
		
	double sumNum = 1;
	double sumDenom = 2;
		
	for(int n = 1; n < INV_TRIG_OPERATION_REP_N; n++)
	{
            tempn = (2*n)+1;
			
            result += (pow(z,tempn) / tempn) * (sumNum/sumDenom);
			
            sumNum *= tempn;
            sumDenom *= tempn+1;
	}

	return result;
    }
    
     /**
     Calculates the arcsine of a triangle with opposite o and hypotenuse h.
     */
    public static double asin2(double o, double h)
    {
        return rawAsin(o/h);
    }
    
     /**
     Calculates the arccosine of a triangle with adjacent a and hypotenuse h.
     * <p>
     * Uses rawAsine for actual operations.
     */
    public static double acos2(double a, double h)
    {
        return asin2(leg(h,a),h);
    }
    
     /**
     Calculates the arcsine of a triangle with opposite o and adjacent a.
     * <p>
     * Uses rawAsine for actual operations.
     */
    public static double atan2(double o, double a)
    {
        return asin2(o,hypot(o,a));
    }
};
